/*
 * Copyright (c) 2010, Davide Brugali, All right reserved
 * 
 * Project MotionPlanning
 * 
 * Package motionplanning.cartspace.framework
 */

//////////////////////////////////////////////////////////////////////////////
//
// File: Transform.h
//
// Purpose: The Transform class acts like a rigid-body transformation, hence
//          it cannot include scaling of objects.
//
//
// Author: Morten Strandberg
//         morten@s3.kth.se
//
//
// Date: 4/8 2003
//
// TODO: Add a function for normalizing a rotation matrix using Gram-Schmidt

package star.core.util.geometry;
import javax.vecmath.Matrix4f;

import star.crust.cartspace.framework.CrtObbMath;

public class HTransform  {
	public Matrix4f matrix;
	public HTransform() {
		matrix = new Matrix4f();
		
		for(int j=0; j<4; j++)
			for(int k=0; k<4; k++)
				if(j == k)
					matrix.setElement(j, k, 1);
				else
					matrix.setElement(j, k, 0);
	}
	
	public float rowdot(int i, float v[])
	{
	return 	matrix.getElement(0,i) * v[0] + matrix.getElement(1, i) * v[1] + matrix.getElement(2,i)*v[2];
	}
	
	public float dot(int i,float  v2[]) {
		float v1[]=new float[4];
		 matrix.getColumn(i, v1);
		  return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
		}
	
	public float[] getMatrixColumn(int i)
	{
		float[] col=new float[4];
		this.matrix.getColumn(i, col);
		return col;
	}
	
	public HTransform setAbsMatrix()
	{
		HTransform ht= new HTransform();
		ht.matrix.setElement(0, 0, Math.abs(this.matrix.m00));
		ht.matrix.setElement(0, 1, Math.abs(this.matrix.m01));
		ht.matrix.setElement(0, 2, Math.abs(this.matrix.m02));
		ht.matrix.setElement(0, 3, Math.abs(this.matrix.m03));
		ht.matrix.setElement(1, 0, Math.abs(this.matrix.m10));
		ht.matrix.setElement(1, 1, Math.abs(this.matrix.m11));
		ht.matrix.setElement(1, 2, Math.abs(this.matrix.m12));
		ht.matrix.setElement(1, 3, Math.abs(this.matrix.m13));
		ht.matrix.setElement(2, 0, Math.abs(this.matrix.m20));
		ht.matrix.setElement(2, 1, Math.abs(this.matrix.m21));
		ht.matrix.setElement(2, 2, Math.abs(this.matrix.m22));
		ht.matrix.setElement(2, 3, Math.abs(this.matrix.m23));
		return ht;
	}
	
	  public FloatXYZ getPointFromHT(FloatXYZ p)
	  {
		  FloatXYZ temp= new FloatXYZ();
		  temp.x=this.rowdot(0, p.getVector())+p.x;
		  temp.y=this.rowdot(1,p.getVector())+p.y;
		  temp.z=this.rowdot(2,p.getVector())+p.z;
		  return temp;
	  }
	  
	  public static HTransform setInvMult(HTransform a, HTransform b)
	  {
		  HTransform newpos= new HTransform();
		  float[] posA=CrtObbMath.HTransform2T(a);
		  float[] posB=CrtObbMath.HTransform2T(b);
		  float[] nVector= new float[3];
		  nVector[0]=posA[0]-posB[0];
		  nVector[1]=posA[1]-posB[1];
		  nVector[2]=posA[2]-posB[2];
		  float vx,vy,vz =0;
		  vx=a.dot(0, nVector); //m03
		  vy=a.dot(1,nVector);  //m13
		  vz=a.dot(2,nVector);  //m23
		  float[] pos={vx,vy,vz};
		  float [][] m3= multT(CrtObbMath.HTransform2R(a),CrtObbMath.HTransform2R(b));
		  newpos=R_T2HTransform(m3,pos);
		  return newpos;
	  }
	  
		public static HTransform R_T2HTransform(float[][] R, float[] T) {
			HTransform ht = new HTransform();
			for(int i=0; i<3; i++) 
				for(int j=0; j<3; j++) ht.matrix.setElement(i, j, R[i][j]);
			for(int i=0; i<3; i++) ht.matrix.setElement(i, 3, T[i]);
			return ht;
		}

	  
	  private static float dotVector(float[] v1, float[] v2)
	  {
		  return v1[0]*v2[0] + v1[1]*v2[1]+v2[2]*v2[2];
	  }
	  
	  
	  public static float[][] multT(float[][] m1, float[][] m2)
	  {
		  float[][] m3= new float[3][3];
		  m3[0][0]=dotVector(m1[0], m2[0]);
		  m3[1][0]=dotVector(m1[1], m2[0]);
		  m3[2][0]=dotVector(m1[2], m2[0]);
		  m3[0][1]=dotVector(m1[0], m2[1]);
		  m3[1][1]=dotVector(m1[1], m2[1]);
		  m3[2][1]=dotVector(m1[2], m2[1]);
		  m3[0][2]=dotVector(m1[0], m2[2]);
		  m3[1][2]=dotVector(m1[1], m2[2]);
		  m3[2][2]=dotVector(m1[2], m2[2]);
		  return m3;

	  }
	  
	public void set(HTransform transform) {
		this.matrix.set(transform.matrix);
	}
	public void product(Matrix4f a, Matrix4f b) {
		this.matrix.mul(a, b);
	}
	public void product(Matrix4f a) {
		this.matrix.mul(a);
	}


	public void translateX(float dist) {
		matrix.m03 += dist;
	}
	public void translateY(float dist) {
		matrix.m13 += dist;
	}
	public void translateZ(float dist) {
		matrix.m23 += dist;
	}
	public void translate(float vector[]) {
		if(vector[0] != 0)		 			// X translation
			matrix.m03 += vector[3];
		else if(vector[1] != 0) 			// Y translation
			matrix.m13 += vector[3];
		else if(vector[2] != 0) 			// Z translation
			matrix.m23 += vector[3];
	}
	
	public void rotateX(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];

		newval[0][0] =  matrix.m01 * cos_a + matrix.m02 * sin_a; 
		newval[0][1] = -matrix.m01 * sin_a + matrix.m02 * cos_a;
		newval[1][0] =  matrix.m11 * cos_a + matrix.m12 * sin_a; 
		newval[1][1] = -matrix.m11 * sin_a + matrix.m12 * cos_a;
		newval[2][0] =  matrix.m21 * cos_a + matrix.m22 * sin_a; 
		newval[2][1] = -matrix.m21 * sin_a + matrix.m22 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m01 = (float) newval[0][0];
		matrix.m02 = (float) newval[0][1];
		matrix.m11 = (float) newval[1][0];
		matrix.m12 = (float) newval[1][1];
		matrix.m21 = (float) newval[2][0];
		matrix.m22 = (float) newval[2][1];
	}

	public void rotateY(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];
	
		newval[0][0] =  matrix.m00 * cos_a - matrix.m02 * sin_a; 
		newval[0][1] =  matrix.m00 * sin_a + matrix.m02 * cos_a;
		newval[1][0] =  matrix.m10 * cos_a - matrix.m12 * sin_a; 
		newval[1][1] =  matrix.m10 * sin_a + matrix.m12 * cos_a;
		newval[2][0] =  matrix.m20 * cos_a - matrix.m22 * sin_a; 
		newval[2][1] =  matrix.m20 * sin_a + matrix.m22 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m00 = (float) newval[0][0];
		matrix.m02 = (float) newval[0][1];
		matrix.m10 = (float) newval[1][0];
		matrix.m12 = (float) newval[1][1];
		matrix.m20 = (float) newval[2][0];
		matrix.m22 = (float) newval[2][1];
	}

	public void rotateZ(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];
	
		newval[0][0] =  matrix.m00 * cos_a + matrix.m01 * sin_a; 
		newval[0][1] = -matrix.m00 * sin_a + matrix.m01 * cos_a;
		newval[1][0] =  matrix.m10 * cos_a + matrix.m11 * sin_a; 
		newval[1][1] = -matrix.m10 * sin_a + matrix.m11 * cos_a;
		newval[2][0] =  matrix.m20 * cos_a + matrix.m21 * sin_a; 
		newval[2][1] = -matrix.m20 * sin_a + matrix.m21 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m00 = (float) newval[0][0];
		matrix.m01 = (float) newval[0][1];
		matrix.m10 = (float) newval[1][0];
		matrix.m11 = (float) newval[1][1];
		matrix.m20 = (float) newval[2][0];
		matrix.m21 = (float) newval[2][1];
	}

	public void rotate(float vector[]) {
		if(vector[0] != 0) 
			rotateX(vector[3]);
		else if(vector[1] != 0) 
			rotateY(vector[3]);
		else if(vector[3] != 0) 
			rotateZ(vector[3]);
	}	
	public String getType() {
		return "matrix";
	}
	public String toString() {
		String doc = "HTransform : \n";
		for(int i=0; i<4; i++) {
			for(int j=0; j<4; j++)
				doc += "  " + matrix.getElement(i,j) + " ";
			doc += "\n";
		}
		return doc;
	}
}
